
#include "TrackPinJoint.h"
#include "World.h"

namespace p2 {

IMPLEMENT_RTT(p2, TrackPinJoint);

TrackPinJoint::~TrackPinJoint()
{
}

TrackPinJoint* TrackPinJoint::create(Anchor const& a0, Anchor const& a1,
		Vector2<real> a, real length, void* userData)
{
	TrackPinJoint* newTrackPinJoint = _allocator.allocate(1);
	if (newTrackPinJoint) {
		_allocator.construct(newTrackPinJoint, TrackPinJoint(a0, a1, a, length, userData));
	}
	return newTrackPinJoint;
}

void TrackPinJoint::preStep(real64 dT)
{
	PinJoint::preStep(dT);
}

bool TrackPinJoint::solvePositions(real64 dT)
{
	computeFutureAnchorPositions(dT);
	/*
	 * work out the length to correct and check if it needs to be corrected
	 */
    if ( _duth <= EPOS ) {
        return true;
    }
    _uth /= _duth;

	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	_invK = Entity::invK(e0, e1, _rAth, _rBth, _uth);

    /*
     * 'An impulse P that eliminates the distance d in a time step of length h must change
     * the relative velocity of the two points by d / h.'
     * => eliminate distance to line
     */
//    real dxth = Math<real>::Clamp(_duth, -MAX_DISTANCE_CORRECTION, +MAX_DISTANCE_CORRECTION);
//    real remove = dxth / (real)dT;
    real remove = _duth / (real)dT;
    Vector2<real> impulse = (remove * _invK) * _uth;
    /*
     * => dx = distance offset of pxBth from length
     */
    if (_flags & FLAGS_ENABLE_SPRING) {
        /*
         * REF: http://gafferongames.com/game-physics/spring-physics/
         * 'A spring-damper system can be modeled as follows: F = - kx - bv
         * where b is the coefficient of damping and v is the relative velocity
         * between the two points connected by the spring'
         */
    	Vector2<real> r = _rAth - _rA0th;
    	real drlth = _restLength - r.length();
        real dxth = Math<real>::Clamp(drlth, -MAX_DISTANCE_CORRECTION, +MAX_DISTANCE_CORRECTION);
        Vector2<real> dv((e1 ? (e1->getV()+_dv[1].getV()) + (e1->getW()+_dv[1].getW()) * _rBth.perp() : Vector2<real>::ZERO)
        		- (e0 ? (e0->getV()+_dv[0].getV()) + (e0->getW()+_dv[0].getW()) * _rAth.perp() : Vector2<real>::ZERO));
        real dvth = -_wa.dot(dv);
        real F = - _k * dxth - _b * dvth;
        impulse += (F * (real)dT * _invK) * _wa;
    }
	/*
	 * apply impulses
	 */
	if ( e0 ) {
		_dv[0].incV(+e0->getInvMass() * impulse);
		_dv[0].incW(+e0->getInvInertia() * _rAth.perp().dot(impulse));
    }
    if ( e1 ) {
    	_dv[1].incV(-e1->getInvMass() * impulse);
    	_dv[1].incW(-e1->getInvInertia() * _rBth.perp().dot(impulse));
    }
    return false;
}

bool TrackPinJoint::solveVelocities(real64 dT)
{
	UNUSED(dT);
	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'With the velocity difference dut = (uBt - uAt) and the matrix
     * K = (Ka + Kb) from the last section an impulse P can
     * be computed which eliminates the velocity difference if it is
     * applied to the points in opposite directions'
     */
    Vector2<real> dv((e1 ? (e1->getV()+_dv[1].getV()) + (e1->getW()+_dv[1].getW()) * _rBth.perp() : Vector2<real>::ZERO)
    		- (e0 ? (e0->getV()+_dv[0].getV()) + (e0->getW()+_dv[0].getW()) * _rAth.perp() : Vector2<real>::ZERO));
    real dvth = dv.dot(_uth);
    if ( Math<real>::FAbs(dvth) <= EVEL ) {
        return true;
    }

	/*
	 * eliminate velocity difference dut
	 */
    real remove = Math<real>::Clamp(dvth, -MAX_VELOCITY_CORRECTION, +MAX_VELOCITY_CORRECTION);
    real mag = remove * _invK;
    Vector2<real> impulse(mag * _uth);
	if ( e0 ) {
		_dv[0].incV(+e0->getInvMass() * impulse);
		_dv[0].incW(+e0->getInvInertia() * _rAth.perp().dot(impulse));
    }
    if ( e1 ) {
    	_dv[1].incV(-e1->getInvMass() * impulse);
    	_dv[1].incW(-e1->getInvInertia() * _rBth.perp().dot(impulse));
    }

    return false;
}

void TrackPinJoint::computeFutureAnchorPositions(real64 dT)
{
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'First an impulse P must be found to satisfy the point position
     * constraint. The impulse P will be applied at point A to the
     * first rigid body and the impulse -P at point B to the second
     * rigid body. These impulses must eliminate the distance
     * between the two points at time t0 + h.'
     * => work future point positions at t0 + h
     */
	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();
	if ( e1 ) {
		_xBth = e1->getX() + (e1->getV()+_dv[1].getV()) * (real)dT;
    	Matrix2<real> rot(e1->getA() + (e1->getW()+_dv[1].getW()) * (real)dT);
        _rBth = _anchor1.getPoint() * rot;
    } else {
    	_xBth = _anchor1.getPoint();
    	_rBth = Vector2<real>::ZERO;
    }
    if ( e0 ) {
    	_xAth = e0->getX() + (e0->getV()+_dv[0].getV()) * (real)dT;
        Matrix2<real> rot(e0->getA() + (e0->getW()+_dv[0].getW())*(real)dT);
        _wa = _a * rot;
        _wla = _wa * _length;
        _rA0th = _anchor0.getPoint() * rot;
        _rA1th = _rA0th+_wla;
        _rAth = Vector2<real>::projectPointOntoEdgeClamped(_xBth+_rBth, _xAth+_rA0th, _xAth+_rA1th) - _xAth;
    } else {
    	_xAth = _anchor0.getPoint();
        _rAth = Vector2<real>::ZERO;
    }
    _uth = _xBth + _rBth - _xAth - _rAth;
    _duth = _uth.length();
}

}
